#include "reset_state.h"
#include "fsm.h"
#include "masters/arm_controller.h"
#include "fsm/pid.h"
#include "Eigen/Eigen"
using namespace std;
using namespace Eigen;
using namespace arm;
ResetState::ResetState()
{

}

/**
 * @brief PositionState::Update
 * @param fsm
 *
 *
 */
void ResetState::Update(FSM *fsm)
{
    //cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" ResetState::Update"<<endl;
    char master_device=fsm->arm_controller_->GetMasterDeviceIndex();


    switch (fsm->arm_controller_->GetRunningStatus())
    {
    case main_controller::kRCPause:
        fsm->SetState(BaseState::kPauseState);
        break;
    case main_controller::kRCStop:
        fsm->SetState(BaseState::kSleepState);
        break;
    case main_controller::kRCWorking:
        if(fsm->arm_controller_->GetConnectionStatus()!=main_controller::SocketConnectionStatusType::kConnectionOkay)
        {
            fsm->SetState(BaseState::kPauseState);
        }
        else
        {
            master_device=fsm->arm_controller_->GetMasterDeviceIndex();
            if(master_device<0)
            {
                fsm->SetState(BaseState::kPauseState);
            }
            else
            {
                if(true)
                {
                    cout<<"arrived"<<endl;
                    fsm->SetState(BaseState::kWorkingState);
                }
                else {
                    cout<<"is moving "<<endl;
                }
            }
        }
        break;
    default:
        break;
    }
}

/**
 * @brief PositionState::OnEntry
 */

void ResetState::OnEntry(FSM *fsm)
{
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" ResetState::OnEntry"<<endl;

    char master_device=fsm->arm_controller_->GetMasterDeviceIndex();
    if(master_device<0)
    {
        fsm->SetState(BaseState::kPauseState);
        return;
    }


//    drdRegulatePos(true,master_device);
//    drdRegulateGrip(true,master_device);

    //Vector3d pos_slave=Vector3d::Zero();
    Matrix4d pose_slave=Matrix4d::Identity();
    pose_slave.topRightCorner(3,1)=fsm->arm_controller_->GetArmCurEndPos();


    Matrix4d pose_master = fsm->arm_controller_->Slave2Master(pose_slave);
    cout<<"pose_master"<<endl<<pose_master<<endl;
//    drdMoveToPos(pose_master(0,3),pose_master(1,3),pose_master(2,3),false,master_device);
//    drdMoveToGrip(0,false,master_device);



//    switch (fsm->GetLastState()) {
//    case BaseState::kPrepareState:
//        //right_device->SetMasterTargetPos(right_device->GetMasterOriginPos());
//        fsm->arm_controller_->SetM2SMappingCoeffLarge();  //set coeff
//        fsm->arm_controller_->master_temp_origin_pos_=fsm->arm_controller_->master_origin_pos_;
//        fsm->arm_controller_->master_target_pos_=fsm->arm_controller_->master_origin_pos_;
//        fsm->arm_controller_->slave_->ResetArmTempOriginPos();
//        fsm->arm_controller_->slave_->SetArmTargetEndPos(fsm->arm_controller_->slave_->GetArmTempOriginPos());
//        break;
//    case BaseState::kWorkingState://case BaseState::PauseState:
//        /**
//         * small->large
//         * coeff turns to the large one
//         * slave_origin_pos turns to slave_origin_pos0
//         * //master_origin_pos turns to master_origin_pos0
//         *
//         * master_target_pos = S2M_MAPPING_COEFF*S2M_TRANSFORM*(slave_target_pos-slave_origin_pos0)+master_origin_pos
//         *
//         *
//         * large->small
//         * coeff turns to the small one
//         * slave_origin_pos turns to slave_target_pos, at which position the arm stays
//         * //master_origin_pos turns to master_origin_pos0
//         *
//         * master_target_pos reset to master_origin_pos0
//         * proof. master_target_pos = S2M_MAPPING_COEFF*S2M_TRANSFORM*(slave_target_pos-slave_origin_pos)+master_origin_pos
//         *                          = S2M_MAPPING_COEFF*S2M_TRANSFORM*(slave_target_pos-slave_target_pos)+master_origin_pos
//         *                          = master_origin_pos
//         */
//        if(fabs(fsm->arm_controller_->GetM2SMappingCoeff()-ArmController::small_m2s_mapping_coeff)<0.01) //small->large
//        {
//            fsm->arm_controller_->SetM2SMappingCoeffLarge();  //set coeff
//            fsm->arm_controller_->master_temp_origin_pos_=fsm->arm_controller_->master_origin_pos_;
//            fsm->arm_controller_->master_target_pos_=fsm->arm_controller_->Slave2Master(fsm->arm_controller_->slave_->GetArmTargetEndPos()); //set master_target_pos
//            fsm->arm_controller_->slave_->ResetArmTempOriginPos(); //set slave_origin_pos
//            //fsm->arm_controller_->slave_->SetTargetEndPos()      slave does not need to set target pos, keep still
//        }
//        else                                                                   //large->small
//        {
//            fsm->arm_controller_->SetM2SMappingCoeffSmall(); //set coeff
//            fsm->arm_controller_->master_temp_origin_pos_=fsm->arm_controller_->master_origin_pos_;
//            fsm->arm_controller_->master_target_pos_=fsm->arm_controller_->master_temp_origin_pos_; //set master_target_pos
//            fsm->arm_controller_->slave_->SetArmTempOriginPos(fsm->arm_controller_->slave_->GetArmTargetEndPos()); //slave_origin_pos
//            //fsm->arm_controller_->slave_->SetTargetEndPos()    slave does not need to set target pos, keep still
//        }
//        break;
//    default:
//        break;
//    }
}

void ResetState::OnExit(FSM *fsm)
{
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" ResetState::OnExit"<<endl;
}
